
#include <iostream>
#include <fstream>
#include <map>
#include "ball_mapping.h"
//#include "ball_motion_analysis.h"

#define MIN_COURT_WIDTH 6
struct Badminton_2d_coord {
    int visibility;
    float x;
    float y;
};

int read_cam_param(com_cam_param_t *cams, int num, const char* intr,const char*extr)
{
    FILE* fp_input_extr = NULL;
    FILE* fp_input_intr = NULL;

    if (extr && strlen(extr) > 0) {
        fp_input_extr = fopen(extr, "r");
        if (fp_input_extr == NULL) {
            printf("Open extrinsic file %s failed\n", extr);
            return -1;
        }
    }
    if (intr && strlen(intr) > 0) {
        fp_input_intr = fopen(intr, "r");
        if (fp_input_intr == NULL) {
            printf("Open intrinsic file %s failed\n", intr);
            return -1;
        }
    }
    if (fp_input_extr == NULL && fp_input_intr == NULL) {
        return -1;
    }

    memset(cams, 0, sizeof(com_cam_param_t) * num);

    for (int i = 0; i < num; i++) {
        int img_id, cam_id; // no used
        int cx, cy;         //no used
        char tmp_str[1024];
        char model_str[64]; // no used

        com_cam_param_t *p = cams + i;

        if (fp_input_extr) {
            char *str = fgets(tmp_str, 1024, fp_input_extr);
            if (str == NULL) {
                printf("Read string failed for %s, cam %d\n", extr, i);
                return -1;
            }
            printf("+=========== tmp_str is %s\n", tmp_str);
            int ret = sscanf(tmp_str, "%d %lf %lf %lf %lf %lf %lf %lf\n", &img_id, &p->QW, &p->QX, &p->QY, &p->QZ, &p->TX, &p->TY, &p->TZ);
            if (ret != 8) {
                printf("Read string failed for %s, cam %d, ret %d\n", extr, i, ret);
                return -1;
            }
            printf("img_id %d, p->TZ %lf\n", img_id, p->TZ);
        }

        if (fp_input_intr) {
            char *str = fgets(tmp_str, 1024, fp_input_intr);
            if (str == NULL) {
                printf("Read string failed for %s\n", intr);
                return -1;
            }
            model_str[0] = 0;
            sscanf(tmp_str, "%d %s \n", &cam_id, model_str);
            printf("======= tmp_str %s, model_str %s\n", tmp_str, model_str);
            if (!strcmp(model_str, "SIMPLE_PINHOLE")) {
                sscanf(tmp_str, "%d %s %d %d %lf %d %d\n", &cam_id, model_str, &p->width, &p->height, &p->k0, &cx, &cy);
                p->k1 = p->k0;
            }
            else if (!strcmp(model_str, "RADIAL_FISHEYE") || !strcmp(model_str, "RADIAL")) {
                sscanf(tmp_str, "%d %s %d %d %lf %d %d %lf %lf\n", &cam_id, model_str, &p->width, &p->height, &p->k0, &cx, &cy, &p->k1, &p->k2);
                p->k1 = p->k0;
            }
            else if (!strcmp(model_str, "PINHOLE")) {
                sscanf(tmp_str, "%d %s %d %d %lf %lf %d %d\n", &cam_id, model_str, &p->width, &p->height, &p->k0, &p->k1, &cx, &cy);
            }
            else {
                printf("cam_model %s is not suport, for cam %d\n", model_str, i);
                return -1;
            }
        }
    }

    if (fp_input_intr) {
        fclose(fp_input_intr);
    }
    if (fp_input_extr) {
        fclose(fp_input_extr);
    }
    printf("read camera param extr:%s,  intr:%s \n", extr, intr);
    return 0;
}

int read_view_corner(ball_map_config_view_t* ball_config_views, const char* corner_fn, int num)
{
    FILE* fp_input_corner = NULL;

    if (corner_fn && strlen(corner_fn) > 0) {
        fp_input_corner = fopen(corner_fn, "r");
        if (fp_input_corner == NULL) {
            printf("Open extrinsic file %s failed\n", corner_fn);
            return -1;
        }
    }
    memset(ball_config_views, 0, sizeof(ball_map_config_view_t) * num);

    for(int i = 0; i<num; i++) {
        char tmp_str[1024];

        char *str = fgets(tmp_str, 1024, fp_input_corner);
        if (str == NULL) {
            printf("Read string failed for %s\n", fp_input_corner);
            return -1;
        }
        ball_map_config_view_t *p = ball_config_views + i;

        //printf("struct p address %p\n", (void*)p);
        //printf("corner_points address %p\n", (void*)p->corner_points);

        // 打印调试信息
        printf("尝试解析字符串: %s\n", tmp_str);
        
        int ret = sscanf(tmp_str, "%d %f %f %f %f %f %f %f %f %f %f %f %f", 
                        &p->view_id, 
                        &p->corner_points[0].x, &p->corner_points[0].y, 
                        &p->corner_points[1].x, &p->corner_points[1].y, 
                        &p->corner_points[2].x, &p->corner_points[2].y, 
                        &p->corner_points[3].x, &p->corner_points[3].y, 
                        &p->basket_rims_point[0].x, &p->basket_rims_point[0].y, 
                        &p->basket_rims_point[1].x, &p->basket_rims_point[1].y);
        printf("success match %d params\n", ret);
        if (ret != 13) {
            printf("Read string failed for %s, cam %d, ret %d\n", fp_input_corner, i, ret);
            return -1;
        }
        printf("===== p->view_id %d \n", p->view_id);
        for (int j=0; j<4; j++){
            p->corner_points[j].x = p->corner_points[j].x/IMG_WIDTH;
            p->corner_points[j].y = p->corner_points[j].y/IMG_HEIGHT;
            printf("p->corner_point[%d] x : %f, y : %f\n", j, p->corner_points[j].x, p->corner_points[j].y);
        }
        for (int j=0; j<2; j++){
            p->basket_rims_point[j].x = p->basket_rims_point[j].x/IMG_WIDTH;
            p->basket_rims_point[j].y = p->basket_rims_point[j].y/IMG_HEIGHT;
            printf("p->basket_rims_point[%d] x : %f, y : %f\n", j, p->basket_rims_point[j].x, p->basket_rims_point[j].y);
        }
    }
    
    printf("Parse ball corner view from %s\n", corner_fn); 
    return 0;
}

void gen_cam_matrix(double matrix_w_2_c[4][4], double QW, double QX, double QY, double QZ, double TX, double TY, double TZ)
{
    matrix_w_2_c[0][0] = 1 - 2 * QY * QY - 2 * QZ * QZ;
    matrix_w_2_c[0][1] =     2 * QX * QY - 2 * QZ * QW;
    matrix_w_2_c[0][2] =     2 * QX * QZ + 2 * QY * QW;

    matrix_w_2_c[1][0] =     2 * QX * QY + 2 * QZ * QW;
    matrix_w_2_c[1][1] = 1 - 2 * QX * QX - 2 * QZ * QZ;
    matrix_w_2_c[1][2] =     2 * QY * QZ - 2 * QX * QW;

    matrix_w_2_c[2][0] =     2 * QX * QZ - 2 * QY * QW;
    matrix_w_2_c[2][1] =     2 * QY * QZ + 2 * QX * QW;
    matrix_w_2_c[2][2] = 1 - 2 * QX * QX - 2 * QY * QY;


    matrix_w_2_c[0][3] = TX;
    matrix_w_2_c[1][3] = TY;
    matrix_w_2_c[2][3] = TZ;

    matrix_w_2_c[3][0] = 0;
    matrix_w_2_c[3][1] = 0;
    matrix_w_2_c[3][2] = 0;
    matrix_w_2_c[3][3] = 1;
}


static void init_camera_matrixs(com_cam_t *cam, double QW, double QX, double QY, double QZ, double TX, double TY, double TZ)
{
    memset(cam->matrix_c_2_w, 0, sizeof(cam->matrix_w_2_c));
    gen_cam_matrix(cam->matrix_w_2_c, QW, QX, QY, QZ, TX, TY, TZ);

    M3x3_Trans(&cam->matrix_w_2_c[0][0], 4, &cam->matrix_c_2_w[0][0], 4);
    M3x3_x_V3_Negative(&cam->matrix_c_2_w[0][0], 4, &cam->matrix_w_2_c[0][3], 4, &cam->matrix_c_2_w[0][3], 4);
    cam->matrix_c_2_w[3][3] = 1;

    //if (TZ > 0){
    //    for (int i = 0; i < 3; i++) {
    //        cam->matrix_c_2_w[i][2] *= -1;
    //        cam->matrix_w_2_c[i][2] *= -1;
    //    }
    //}
}

/**
 * 完整的ball_map_ctx_t初始化函数
 */
ball_map_ctx_t* initialize_ball_map_context(
    const char* intrinsic_file, 
    const char* extrinsic_file,
    const char* corner_file,
    int output_stream_id = 0)
{
    const int cam_num = 36;  // 根据实际情况调整
    const int view_cnt = 8;  // 根据实际情况调整
    
    // 步骤1: 读取相机参数
    com_cam_param_t* cams_param = (com_cam_param_t*)malloc(sizeof(com_cam_param_t) * cam_num);
    com_cam_t* cams = (com_cam_t*)malloc(sizeof(com_cam_t) * cam_num);
    
    if (read_cam_param(cams_param, cam_num, intrinsic_file, extrinsic_file) != 0) {
        printf("Failed to read camera parameters\n");
        free(cams_param);
        free(cams);
        return nullptr;
    }
    
    // 初始化相机矩阵
    for (int i = 0; i < cam_num; i++) {
        init_camera_matrixs(cams + i, 
                           cams_param[i].QW, cams_param[i].QX, cams_param[i].QY, cams_param[i].QZ,
                           cams_param[i].TX, cams_param[i].TY, cams_param[i].TZ);
        cams[i].width = cams_param[i].width;
        cams[i].height = cams_param[i].height;
        cams[i].fx = cams_param[i].k0;
        cams[i].fy = cams_param[i].k1;
    }
    
    // 步骤2: 读取视角配置
    ball_map_config_view_t* ball_config_views = (ball_map_config_view_t*)malloc(sizeof(ball_map_config_view_t) * view_cnt);
    if (read_view_corner(ball_config_views, corner_file, view_cnt) != 0) {
        printf("Failed to read view corner configuration\n");
        free(cams_param);
        free(cams);
        free(ball_config_views);
        return nullptr;
    }
    
    // 步骤3: 设置归一化的角点坐标（用于透视变换）
    // 这些坐标定义了输出视角的边界
    point_t norm_corner_points[4] = {
        {0.0f, 1.0f},  // 左上
        {1.0f, 1.0f},  // 右上  
        {1.0f, 0.0f},  // 右下
        {0.0f, 0.0f}   // 左下
    };

    
    // 步骤4: 创建配置结构体
    ball_map_config_t config;
    config.cams = cams;
    config.cfg_views = ball_config_views;
    config.view_cnt = view_cnt;
    config.out_stream_id = output_stream_id;
    config.norm_corner_points = norm_corner_points;
    
    // 步骤5: 创建羽毛球映射上下文
    ball_map_ctx_t* ctx = create_ball_map(config);
    
    // 清理临时内存（注意：cams和ball_config_views现在由ctx管理）
    free(cams_param);
    
    if (!ctx) {
        printf("Failed to create ball map context\n");
        return nullptr;
    }
    
    printf("Ball map context initialized successfully\n");
    printf("Output stream: %d\n", ctx->out_stream_id);
    printf("Camera unit: (%f, %f, %f)\n", ctx->cam_unit_x, ctx->cam_unit_y, ctx->cam_unit_z);
    
    return ctx;
}

int parse_2d_ball(std::string coord_2d_file, std::map<int, Badminton_2d_coord>& frame_2_coord)
{
    std::ifstream file(coord_2d_file);
    std::string line;

    std::getline(file, line);
    printf("=== ignore first line %s\n", line);

    while(std::getline(file, line)){
        //std::cout << line << std::endl;
        std::stringstream ss(line);
        std::vector<std::string> fileds;
        std::string token;
        while(std::getline(ss, token, ',')){
            fileds.push_back(token);
        }
        if(fileds.size() == 4){
            int frame_idx = std::stoi(fileds[0]);
            Badminton_2d_coord coord;
            coord.visibility = std::stoi(fileds[1]);
            coord.x = (std::stoi(fileds[2])+0.0)/IMG_WIDTH;
            coord.y = (std::stoi(fileds[3])+0.0)/IMG_HEIGHT;
            frame_2_coord.insert(std::make_pair(frame_idx, coord)); 
        }
        else{
            std::cout << "Invalid line " << line << std::endl;
        }
    }
    file.close();
    return 0;
}

int trans_view_2d_to_3d(ball_map_ctx_t* ball_ctx, int case_idx){
    //std::vector<int> view_select = {0, 4, 9, 13, 18, 22, 27, 32};
    //std::vector<int> view_select = {0, 4, 13, 18, 22, 32};
    std::vector<int> view_select = {0, 1, 2, 17, 18, 19, 34, 35};


    std::vector<std::map<int, Badminton_2d_coord>> view_frame_2_coord;

    //BallMotionAnalyzer motion_analyzer(15, 3.0f, 8.0f, 45.0f);

    //int case_idx = 4;
    for(auto it=view_select.begin(); it!=view_select.end();++it){
        int view = *it;
        std::stringstream coord_file;
        coord_file << "2d_tracknet_det/case_"<< case_idx <<"_v_" << view << "_ball.csv";
        std::cout << "init coord from " << coord_file.str() << std::endl;
        std::map<int, Badminton_2d_coord> frame_2_coord;
        parse_2d_ball(coord_file.str(), frame_2_coord);
        view_frame_2_coord.push_back(frame_2_coord);
    }
    //std::cout << "==== view frame_2_coord " << view_frame_2_coord.size() << std::endl;
    int frame_cnt = view_frame_2_coord[0].size();
    std::cout << "=== total line cnt " << frame_cnt << std::endl;

    std::map<int, ball_info_t> frame_2_ball3d;
    //std::vector<hit_point_t> all_hit_points;
    for(int frame_idx = 0; frame_idx < frame_cnt;frame_idx++){
        ball_result_t detections[view_select.size()];
        int view_availble_cnt = 0;
        for(int v_id=0; v_id<view_select.size(); v_id++){
            std::map<int, Badminton_2d_coord> frame_2_coord = view_frame_2_coord[v_id];
            Badminton_2d_coord view_coord = frame_2_coord[frame_idx];
            detections[v_id].rect = {view_coord.x, view_coord.y, 1/IMG_WIDTH, 1/IMG_HEIGHT};
            if(view_coord.visibility == 1.0){
                detections[v_id].score = 1.0;
                view_availble_cnt += 1;
            } else {
                detections[v_id].score = 0.0;
            }
        }

        ball_info_t ball_info;
        if(view_availble_cnt >= 2) {
            std::cout << "============ check frame " << frame_idx << "=============" << std::endl;
            int result = get_ball_coord_by_views(ball_ctx, detections, &ball_info);
            //std::cout << "ball_info availble " << ball_info.avail << std::endl;
            //printf("Ball 3D position: (%f, %f, %f)\n", ball_info.court_relative_pos.x, ball_info.court_relative_pos.y, ball_info.court_relative_pos.z);
            if(ball_info.court_relative_pos.x < (-1*MIN_COURT_WIDTH) or ball_info.court_relative_pos.x > MIN_COURT_WIDTH or ball_info.court_relative_pos.z < 0 or ball_info.court_relative_pos.z > 4 or ball_info.court_relative_pos.y < -8 or ball_info.court_relative_pos.y > 8)
            {
                printf("====== ignore out %f, %f, %f\n", ball_info.court_relative_pos.x, ball_info.court_relative_pos.y, ball_info.court_relative_pos.z);
                ball_info.avail = 0;
            }// else {
             //   motion_analyzer.addMotionData(frame_idx, ball_info);
             //}
   
        } else {
            ball_info.avail = 0;
            printf("not enough 2d view for frame %d\n", frame_idx);
        }
        frame_2_ball3d.insert(std::make_pair(frame_idx, ball_info));
        //motion_analyzer.addMotionData(frame_idx, ball_info);
        //if( frame_idx % 10 ==0 && frame_idx > 10 ) {
        //    std::vector<hit_point_t> hits = motion_analyzer.detectHitPoints();
        //    all_hit_points.insert(all_hit_points.end(), hits.begin(), hits.end());
        //}
    }
    
    // 最终检测所有击球点
    //std::vector<hit_point_t> final_hits = motion_analyzer.detectHitPoints();
    //all_hit_points.insert(all_hit_points.end(), final_hits.begin(), final_hits.end());
    
    // 输出击球点结果
    //printf("\n=== FINAL HIT POINT ANALYSIS ===\n");
    //printf("Total hit points detected: %zu\n", all_hit_points.size());
    //
    //for (const auto& hit : all_hit_points) {
    //    printf("Hit at frame %d: pos(%.3f, %.3f, %.3f), confidence: %.3f\n",
    //           hit.frame_idx, hit.position.x, hit.position.y, hit.position.z, hit.confidence);
    //}

    // 输出击球点文件
    //std::map<int, bool> frame_idx_2_hit;
    //std::stringstream hit_out_file;
    //hit_out_file << "case_" << case_idx << "_hit_points.txt";
    //std::ofstream hit_file(hit_out_file.str());
    //if (hit_file.is_open()) {
    //    hit_file << "frame_idx,x,y,z,confidence,speed_change\n";
    //    for (const auto& hit : all_hit_points) {
    //        if(frame_idx_2_hit.find(hit.frame_idx) != frame_idx_2_hit.end()){
    //            continue;
    //        }
    //        hit_file << hit.frame_idx << "," 
    //                << hit.position.x << "," 
    //                << hit.position.y << "," 
    //                << hit.position.z << ","
    //                << hit.confidence << ","
    //                << hit.speed_change << "\n";
    //        frame_idx_2_hit.insert(std::make_pair(hit.frame_idx, true));
    //    }
    //    hit_file.close();
    //}

    std::stringstream out_file;
    out_file << "2d_tracknet_det/select_" << case_idx << "_v_" << view_select.size() << "_3d.txt";
    std::ofstream file(out_file.str());
    if(!file.is_open()){
        std::cerr << "can not open file to write" << std::endl;
        return -1;
    }
    file << "frame_idx,visible,x,y,z" << std::endl;
    for(int f_idx = 0; f_idx<frame_cnt; f_idx++){
        ball_info_t ball_info = frame_2_ball3d[f_idx];
        if(ball_info.avail != 0){
            file << f_idx << ",1," << ball_info.court_relative_pos.x << "," << ball_info.court_relative_pos.y << "," << ball_info.court_relative_pos.z << std::endl;
        }
        else{
            file << f_idx << ",0,0,0,0" << std::endl;
        }
    }
    
    return 0;

}

int trans_corner_2d_to_3d(ball_map_ctx_t* ball_ctx,const char* corner_file){
    int view_cnt = 8;
    ball_map_config_view_t* ball_config_views = (ball_map_config_view_t*)malloc(sizeof(ball_map_config_view_t) * view_cnt);
    if (read_view_corner(ball_config_views, corner_file, view_cnt) != 0) {
        std::cerr << "Invalid read corner file failed " << corner_file << std::endl;
        return -1;
    }
    std::map<int, ball_info_t> corner_idx_2_ball3d;

    //printf("=== Coordinate System Verification ===\n");

    for(int corner_idx = 0; corner_idx<4; corner_idx++){
        ball_result_t detections[view_cnt];

        printf("Corner[%d]: (%.3f, %.3f, %.3f)\n", 
                   corner_idx, ball_ctx->points[corner_idx].x, ball_ctx->points[corner_idx].y, ball_ctx->points[corner_idx].z);
        // 验证基础向量
        printf("Basis vectors:\n");
        printf("vec_x: (%.6f, %.6f, %.6f) len: %.3f\n", 
               ball_ctx->vec_x.x, ball_ctx->vec_x.y, ball_ctx->vec_x.z, ball_ctx->vec_x_len);
        printf("vec_y: (%.6f, %.6f, %.6f) len: %.3f\n", 
               ball_ctx->vec_y.x, ball_ctx->vec_y.y, ball_ctx->vec_y.z, ball_ctx->vec_y_len);
        printf("vec_z: (%.6f, %.6f, %.6f)\n", 
               ball_ctx->vec_z.x, ball_ctx->vec_z.y, ball_ctx->vec_z.z);
        
        // 验证正交性
        printf("Orthogonality check:\n");
        printf("x·y: %.6f (should be ~0)\n", dot(ball_ctx->vec_x, ball_ctx->vec_y));
        printf("x·z: %.6f (should be ~0)\n", dot(ball_ctx->vec_x, ball_ctx->vec_z));
        printf("y·z: %.6f (should be ~0)\n", dot(ball_ctx->vec_y, ball_ctx->vec_z));

        for(int v_idx = 0; v_idx<view_cnt; v_idx++){
            ball_map_config_view_t* config_view = ball_config_views + v_idx;
            //std::cout << "===view_idx " << v_idx << " corner_idx[" << corner_idx << "].x " << config_view->corner_points[corner_idx].x << " .y " << config_view->corner_points[corner_idx].y << std::endl;
            float norm_x = config_view->corner_points[corner_idx].x; 
            float norm_y = config_view->corner_points[corner_idx].y;
            detections[v_idx].rect = {norm_x, norm_y, 1/IMG_WIDTH, 1/IMG_HEIGHT};
            //std::cout << "=== input detection norm_x " << norm_x << " norm_y " << norm_y << std::endl;
            detections[v_idx].score = 1.0;
        }
        ball_info_t ball_info;
        int result = get_ball_coord_by_views(ball_ctx, detections, &ball_info);
        if(result!=0){
            std::cerr << "get corner 3d coord error " << std::endl;
            return result;
        }

        corner_idx_2_ball3d.insert(std::make_pair(corner_idx, ball_info));
    }
    std::stringstream out_file;
    out_file << "corner_3d.txt";
    std::ofstream file(out_file.str());
    if(!file.is_open()){
        std::cerr << "can not open file to write" << std::endl;
        return -1;
    }
    file << "frame_idx,visible,x,y,z" << std::endl;
    for(int f_idx = 0; f_idx<4; f_idx++){
        ball_info_t ball_info = corner_idx_2_ball3d[f_idx];
        if(ball_info.avail != 0){
            file << f_idx << ",1," << ball_info.court_relative_pos.x << "," << ball_info.court_relative_pos.y << "," << ball_info.court_relative_pos.z << std::endl;
        }
        else{
            file << f_idx << ",0,0,0,0" << std::endl;
        }
    }
    return 0;
}

int trans_net_center_to_3d(ball_map_ctx_t* ball_ctx,const char* corner_file){
    int view_cnt = 6;
    ball_map_config_view_t* ball_config_views = (ball_map_config_view_t*)malloc(sizeof(ball_map_config_view_t) * view_cnt);
    if (read_view_corner(ball_config_views, corner_file, view_cnt) != 0) {
        std::cerr << "Invalid read corner file failed " << corner_file << std::endl;
        return -1;
    }
    std::map<int, ball_info_t> net_center_2_3d;

    //printf("=== Coordinate System Verification ===\n");

    for(int net_idx = 0; net_idx<2; net_idx++){
        ball_result_t detections[view_cnt];

        printf("rim_center[%d]: (%.3f, %.3f, %.3f)\n", 
                   net_idx, ball_ctx->rim_points[net_idx].x, ball_ctx->rim_points[net_idx].y, ball_ctx->rim_points[net_idx].z);
        // 验证基础向量
        printf("Basis vectors:\n");
        printf("vec_x: (%.6f, %.6f, %.6f) len: %.3f\n", 
               ball_ctx->vec_x.x, ball_ctx->vec_x.y, ball_ctx->vec_x.z, ball_ctx->vec_x_len);
        printf("vec_y: (%.6f, %.6f, %.6f) len: %.3f\n", 
               ball_ctx->vec_y.x, ball_ctx->vec_y.y, ball_ctx->vec_y.z, ball_ctx->vec_y_len);
        printf("vec_z: (%.6f, %.6f, %.6f)\n", 
               ball_ctx->vec_z.x, ball_ctx->vec_z.y, ball_ctx->vec_z.z);
        
        // 验证正交性
        printf("Orthogonality check:\n");
        printf("x·y: %.6f (should be ~0)\n", dot(ball_ctx->vec_x, ball_ctx->vec_y));
        printf("x·z: %.6f (should be ~0)\n", dot(ball_ctx->vec_x, ball_ctx->vec_z));
        printf("y·z: %.6f (should be ~0)\n", dot(ball_ctx->vec_y, ball_ctx->vec_z));

        for(int v_idx = 0; v_idx<view_cnt; v_idx++){
            ball_map_config_view_t* config_view = ball_config_views + v_idx;
            //std::cout << "===view_idx " << v_idx << " net_idx[" << net_idx << "].x " << config_view->basket_rims_point[net_idx].x << " .y " << config_view->basket_rims_point[net_idx].y << std::endl;
            float norm_x = config_view->basket_rims_point[net_idx].x; 
            float norm_y = config_view->basket_rims_point[net_idx].y;
            detections[v_idx].rect = {norm_x, norm_y, 1/IMG_WIDTH, 1/IMG_HEIGHT};
            //std::cout << "=== input detection norm_x " << norm_x << " norm_y " << norm_y << std::endl;
            detections[v_idx].score = 1.0;
        }
        ball_info_t ball_info;
        int result = get_ball_coord_by_views(ball_ctx, detections, &ball_info);
        if(result!=0){
            std::cerr << "get corner 3d coord error " << std::endl;
            return result;
        }

        net_center_2_3d.insert(std::make_pair(net_idx, ball_info));
    }
    std::stringstream out_file;
    out_file << "netcenter_3d.txt";
    std::ofstream file(out_file.str());
    if(!file.is_open()){
        std::cerr << "can not open file to write" << std::endl;
        return -1;
    }
    file << "frame_idx,visible,x,y,z" << std::endl;
    for(int f_idx = 0; f_idx<2; f_idx++){
        ball_info_t ball_info = net_center_2_3d[f_idx];
        if(ball_info.avail != 0){
            file << f_idx << ",1," << ball_info.court_relative_pos.x << "," << ball_info.court_relative_pos.y << "," << ball_info.court_relative_pos.z << std::endl;
        }
        else{
            file << f_idx << ",0,0,0,0" << std::endl;
        }
    }
    return 0;

}

int main(int argc, char *argv[])
{
    const char* intr_fn = "intrinsic.txt";
    const char* extr_fn = "extrinsic.txt";
    
    const char* corner_fn = "corner_res_focus_center_view.txt";
    std::cout << "程序名称:" << argv[0] << std::endl;
    std::cout << "参数个数:" << argc -1 << std::endl;
    for (int i=1; i< argc; ++i){
        std::cout << "参数" << i << ":" << argv[i] << std::endl;
    }
    int case_idx = std::stoi(argv[1]);

    ball_map_ctx_t* ball_ctx = initialize_ball_map_context(intr_fn, extr_fn, corner_fn);
    if (!ball_ctx) {
        printf("Failed to initialize ball mapping context\n");
        return -1;
    } 
    //return trans_net_center_to_3d(ball_ctx, corner_fn);
    return trans_view_2d_to_3d(ball_ctx, case_idx);    
    //return trans_corner_2d_to_3d(ball_ctx, corner_fn);
}

